

% ************************************************************************ 
%     Module name   : Fusion Simulation - scheduleTargets                                                     
%                                                                          
%     Description   : This will schedule the sequence of when the radars will simultaniously intercept the targets in the scene.                                                    
%                                                                          
%     $Header: /Lynx/SimulationFramework/RSP/IFS/IFSRun.m 2     15/10/08 16:08 Lwabeke $ 
%                                                                          
%    $Author: Lwabeke $	            
%                                                                         
%    $Revision: 2 $                
%                                                                         
% ************************************************************************
%     $History: IFSRun.m $ 
% 
%
% ************************************************************************

function [radarSetup] = parseSchedule(currentTime, radarState, TargetSequence, TargetAngles, ActiveTargetSequence, param)

for radarCntr=1:param.num_radars
    ActiveTargetAngles(1, radarCntr) = 0 ; % radarState.radar(radarCntr).posAngle; is already subtracted in predictIntercepts

    ActiveTargetAngles(2:(1+size(ActiveTargetSequence, 1)), radarCntr) = TargetAngles(ActiveTargetSequence,radarCntr);

    ActiveTargetAngles(size(ActiveTargetSequence,1)+2, radarCntr) = sign(ActiveTargetAngles(size(ActiveTargetSequence,1)+1, radarCntr)) * pi/2;     % Finish turn through 90 degrees, using sign of last target to determine direction of turn
end


deltaAngles = diff(ActiveTargetAngles);

maxDeltaAngles = max(abs(deltaAngles), [], 2);

maxDeltaAngles = max(maxDeltaAngles, 1e-6);     % Prevent divide by 0 if both deltas are 0

angleRates = deltaAngles ./ repmat(maxDeltaAngles , 1, 2);

deltaTime = deltaAngles ./( param.radarSlewRate .* angleRates );

deltaTime(isnan(deltaTime(:,1)),1) = deltaTime(isnan(deltaTime(:,1)),2);
deltaTime(isnan(deltaTime(:,2)),2) = deltaTime(isnan(deltaTime(:,2)),1);

deltaTime(isnan(deltaTime)) = 0.0; % No movement, so no delta time needed

if (sum(isnan(deltaTime)) > 0)
    deltaAngles
    deltaTime
    
end

radarSetup.deltaTime = deltaTime;
radarSetup.angleRates = angleRates * param.radarSlewRate;
radarSetup.numSegments = size(deltaAngles, 1);